/*
 Original: David Fritz, 3.24.2010
 Modified: Jeff Blount, 2011
 
 2-Way Basestation Relay for Mercury Robot
 
 usage: ./bs <serial port>
 
 The software listens and trasmits on server port, 31337.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <errno.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>

struct command_Packet {
    unsigned char   ReceiveByte;
    unsigned char   command;
    unsigned char   leftSpeed;
    unsigned char   rightSpeed;
    unsigned char   leftDistanceHigh;
    unsigned char   leftDistanceLow;
    unsigned char   rightDistanceHigh;
    unsigned char   rightDistanceLow;
};

struct feedback_Packet {
    unsigned char   SendByte;
    unsigned char   infraredLeftHigh;
    unsigned char   infraredLeftLow;
    unsigned char   infraredRightHigh;
    unsigned char   infraredRightLow;
    unsigned char   ultrasonicHigh;
    unsigned char   ultrasonicLow;
    unsigned char   leftCurrentHigh;
    unsigned char   leftCurrentLow;
    unsigned char   rightCurrentHigh;
    unsigned char   rightCurrentLow;
    unsigned char   leftEncoderHigh;
    unsigned char   leftEncoderLow;
    unsigned char   rightEncoderHigh;
    unsigned char   rightEncoderLow;
};

#define PORT 31337

void handle_receive_packet(struct command_Packet p, int serialfd)
{
    if (p.ReceiveByte != 86)
    {
        printf("[warning] Invalid Packet with Receive Byte %x\n",p.ReceiveByte);
        fflush(stdout);
    }
    else
    {
        ssize_t temp = write(serialfd, &p, sizeof(struct command_Packet));
	tcflush(serialfd, TCIFLUSH);
    }
}

void handle_send_packet(struct feedback_Packet *q, int serialfd)
{
	unsigned char buffer[11];       
	unsigned char buff;
	int i;
	i=0;
	//printf("[i] looking for Data");	
	//fflush(stdout);
	do{
	ssize_t temp2 = read(serialfd, &buff, 1);
		if(temp2>0){
		buffer[i]=buff;	
        //printf("[i] Received Byte No# %d Data %d\n",i,buffer[i]);
        //fflush(stdout);
		i++;
		}

	}while(i<7);

    q->SendByte=buffer[0];
    q->infraredLeftHigh=buffer[1];
    q->infraredLeftLow=buffer[2];
    q->infraredRightHigh=buffer[3];
    q->infraredRightLow=buffer[4];
    q->ultrasonicHigh=buffer[5];
    q->ultrasonicLow=buffer[6];
	q->leftCurrentHigh=buffer[7];
    q->leftCurrentLow=buffer[8];
    q->rightCurrentHigh=buffer[9];
    q->rightCurrentLow=buffer[10];
    q->leftEncoderHigh=buffer[11];
    q->leftEncoderLow=buffer[12];
    q->rightEncoderHigh=buffer[13];
    q->rightEncoderLow=buffer[14];
}

int main(int argc, char *argv[])
{
    int junk;     // catches return values from functions that i will not test for
    int sockfd, newsockfd;
    socklen_t clilen;
    struct sockaddr_in serv_addr, cli_addr;
    time_t oldTime;
    
    int serialfd,c,res;
    struct termios oldtio, newtio;
    
    if (argc != 2)
    {
        printf("Usage: ./bs <serial device>\n");
        fflush(stdout);
        exit(-1);
    }
    
    printf("[i] TeamVictory Bot Base Station\n");
    fflush(stdout);
    
    printf("[i] Opening Serial Device\n");
    fflush(stdout);
    serialfd = open(argv[1], O_RDWR | O_NOCTTY);
    if (serialfd < 0)
        
    {
        printf("[error] Opening Serial Device\n");
        fflush(stdout);
        exit(-1);
    }
    
    tcgetattr(serialfd, &oldtio);
    memset(&newtio, 0, sizeof(struct termios));
    
    newtio.c_cflag = B9600 | CS8 | CLOCAL | CREAD;
    newtio.c_iflag = IGNPAR | ICRNL;
    newtio.c_oflag = 0;
    //newtio.c_lflag = ICANON;
    
    tcflush(serialfd, TCIFLUSH);
    tcsetattr(serialfd, TCSANOW, &newtio);
    
    printf("[i] Opening Socket\n");
    fflush(stdout);
    sockfd = socket(AF_INET, SOCK_STREAM, 0);
    
    if (sockfd < 0)
    {
        printf("[error] Opening Socket\n");
        fflush(stdout);
        exit(-1);
    }
    
    memset(&serv_addr, 0, sizeof(serv_addr));
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = INADDR_ANY;
    serv_addr.sin_port = htons(PORT);
    if (bind(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0)
    {
        printf("[error] Bind\n");
        fflush(stdout);
        exit(-1);
    }
    
    listen(sockfd,1);
    clilen = sizeof(struct sockaddr_in);
    while (1)
    {
        struct command_Packet p;
        struct feedback_Packet q;
        ssize_t size;
        
        printf("[i] Listening for Clients\n");
        fflush(stdout);
        
        newsockfd = accept(sockfd, (struct sockaddr*) &cli_addr, &clilen);        // Grab a Connection
        if (newsockfd < 0)
        {
            printf("[error] Accept\n");
            fflush(stdout);
            exit(-1);
        }
        printf("[i] Client Connected\n");
        fflush(stdout);        
	oldTime = time(NULL);
        
	while ((size = recv(newsockfd, &p, sizeof(struct command_Packet), MSG_WAITALL)) == sizeof(struct command_Packet))     // Grab Data
        {
            	printf("[received] %c,%c,%c,%c,%c,%c,%c,%c\n",p.ReceiveByte,p.command,p.leftSpeed,p.rightSpeed,p.leftDistanceHigh,p.leftDistanceLow,p.rightDistanceHigh,p.rightDistanceLow);
		fflush(stdout);           	
		handle_receive_packet(p,serialfd);
		handle_send_packet(&q,serialfd);
		size = send(newsockfd, &q, sizeof(struct feedback_Packet), MSG_WAITALL);// Send Data
        	printf("[sent] %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",q.SendByte,q.infraredLeftHigh,q.infraredLeftLow,q.infraredRightHigh,q.infraredRightLow,q.ultrasonicHigh,q.ultrasonicLow,q.leftCurrentHigh,q.leftCurrentLow,q.rightCurrentHigh,q.rightCurrentLow,q.leftEncoderHigh,q.leftEncoderLow,q.rightEncoderHigh,q.rightEncoderLow);
            fflush(stdout);
        }
               
        printf("[warning] Dropped Connection!\n");      // Connection Lost
        fflush(stdout);
        junk = close(newsockfd);
    }
    
    return 0;
}

